/*
 * ObstacleAvoider.cpp
 *
 *  Created on: Apr 2, 2014
 *      Author: ofir_oleg
 */

#include <unistd.h>
#include <signal.h>

#include <cmath>
#include "ObstacleAvoider.h"
#include "../../utilities/Constants.h"

#define INIT_DELAY 1 // delay (seconds) to initialize system
#define SUBS_DELAY 2 // delay (seconds) to apply subscriptions
#define EMERGENCY_STOP 0.35 // minimal distance (meters) to obstacle which causes robot to stop
#define MIN_RIGHT_OBSTACLE 0 // ignore right side TODO
#define MIN_CENTER_OBSTACLE 1.5 // reaction distance to obstacle ahead
#define MIN_LEFT_OBSTACLE 0 // ignore left side TODO
#define CENTER 360 // center index of laser scan
#define RIGHT_RANGE_START 360 // start point on laser scan's array while reading to the right
#define RIGHT_RANGE_END 100 // right-most point on laser scan's array
#define LEFT_RANGE_START 360 // start point on laser scan's array while reading to the left
#define LEFT_RANGE_END 620 // left-most point on laser scan's array
#define RESPONSE_TIME 1 // seconds
#define DIFF_TO_ANGLE 100.0 // normalizes index differences on laser scan to turn angles
#define LINEAR_BOOST 0.05 // added to bypass angle
#define BOOST_BYPASS_ANGLE 5.5 // scales up turn angle while bypassing
#define BOOST_RESET_ANGLE 5 // // scales up reset turn after bypassing
#define STOP_PREV_TURN 0.15 // TODO 0.1 // how much should turn in order to stop previously published turn
#define SIGNIFICAT_DIST_DIFF 1.0 // difference between points on range which indicates obstacle's edge

// TODO DELETE IF NOT IMPLEMENTING ORIENTATION FIX
#define WINDOW_SIZE 10
#define LEFT_SIGN 1
#define RIGHT_SIGN -1
#define TOLERANT 0.005 // TODO 0.02
//

enum State {
	START,
	BYPASS,
	IN_BYPASS,
	STOP_BYPASS,
	MAINTAIN,
	RESET_ORIENTAION,
	STOP_RESET
};
short curr_state = START; // current state in automaton implementation // TODO EXPLAIN IN doxygen manual

ObstacleAvoider::ObstacleAvoider(Driver* driver, double speed, bool run_once) {

	m_loop_rate = new ros::Rate(10);

	m_driver = driver;
	m_speed = speed;
	this->m_run_once = run_once;

	m_right_bin = 0.0;
	m_center_bin = 0.0;
	m_left_bin = 0.0;

	// subscribe to sensors module

	int queue_depth = 1000;

	m_sensors_publisher = m_node_handler.advertise<std_msgs::String>(
			"/Sensors_Module/Subscribers_Channel", 1000);

	// subscribe to front laser
	sub_sensors_fl = m_node_handler.subscribe(
			"/Sensors_Module/Front_Laser_Range", queue_depth,
			&ObstacleAvoider::frontLaserHandler, this);

	// force connection establishment
	m_loop_rate->sleep();
	sleep(INIT_DELAY);
}

ObstacleAvoider::~ObstacleAvoider() {
	// TODO Auto-generated destructor stub
}

void ObstacleAvoider::subscribeToSensor() {

	ROS_INFO("in ObstacleAvoider::subscribeToSensor"); // TODO

	// compose message
	std_msgs::String m_message_to_publish;
	char buffer[32];
	snprintf(buffer, sizeof(buffer), "%d%s%d%s%f%s%f",
			sensors_names::FRONT_LASER_RANGE, DELIMITER, int(true), DELIMITER,
			0.0, DELIMITER, 720.0); // TODO WRITE 0,720 AS CONSTANTS
	m_message_to_publish.data = buffer;

	// publish the message to sensors
	m_sensors_publisher.publish(m_message_to_publish);
	sleep(SUBS_DELAY);

	ROS_INFO("in ObstacleAvoider::subscribeToSensor- published"); // TODO
}

void ObstacleAvoider::frontLaserHandler(
		const std_msgs::Float32MultiArray::ConstPtr& range) {

	// calculate average distance for each bin
	float right = avgElements(range, 180, 200);
	float center = avgElements(range, 350, 370);
	float left = avgElements(range, 520, 540);

	// set the values safely
	setBins(right, center, left);

	// set all the range safely
	setRange(range);
}

float ObstacleAvoider::avgElements(
		const std_msgs::Float32MultiArray::ConstPtr& range, int start,
		int end) {

	// sum elements
	float sum = 0.0;
	for (int i = start; i < end; ++i) {
		sum += range->data[i];
	}

	// return average
	return sum / (end - start + 1);
}

float ObstacleAvoider::avgElements(const float range[], int start, int end) {

	// sum elements
	float sum = 0.0;
	for (int i = start; i < end; ++i) {
		sum += range[i];
	}

	// return average
	return sum / (end - start + 1);
}

/*
 * Signal handler is called when countdown is over
 */
void alarmHandler(int signum) {

	switch (curr_state) {

	case MAINTAIN: {
		ROS_INFO("ALRM UP- MAINTAIN");
		// passed obstacle, fix orientation back to original
		curr_state = RESET_ORIENTAION;
		break;
	}

	default:
		break;
	}
}

void ObstacleAvoider::run() {

	// subscribe to get data from sensor
	subscribeToSensor();

	// register to alarm signal
	struct sigaction sact;
	sigemptyset(&sact.sa_mask);
	sact.sa_flags = 0;
	sact.sa_handler = alarmHandler;
	sigaction(SIGALRM, &sact, NULL);

	// approximates how long the bypass turn was
	unsigned long counter = 0;

	// when reaches on obstacle, bypass it
	float turn_angle = 0.0, obstacle_edge = 0.0;
	while (ros::ok()) {
		ros::spinOnce();

		// check for emergency stop
		if (checkObstacle(EMERGENCY_STOP,EMERGENCY_STOP,EMERGENCY_STOP)) {
			ROS_ERROR("In ObstacleAvoider::run() - EMERGENCY_STOP");
			m_driver->stop();
			sleep(1);
			curr_state = START;
			continue;
		}

		// act according to current state
		switch (curr_state) {

		case START: {

			m_driver->move(m_speed, 0.0);

			// if obstacle is near, bypass it
			if (checkObstacle(MIN_RIGHT_OBSTACLE, MIN_CENTER_OBSTACLE, MIN_LEFT_OBSTACLE)) {

				// fix robot's orientation TODO
//				fixOrientation();

				// bypass obstacle when robot faces it straight
				curr_state = BYPASS;
				ROS_INFO("START->BYPASS");
			}

			break;
		}

		case BYPASS: {

			// plan route to bypass nearest obstacle
			bypassObstacle(turn_angle, obstacle_edge);
			curr_state = IN_BYPASS;
			ROS_INFO("BYPASS->IN_BYPASS"); // TODO
			break;
		}

		case IN_BYPASS: {

			// read current distances at front
			float range[MAX_RANGE_SIZE];
			getRange(range);
			float prevDist = range[CENTER], currDist = range[CENTER];

			ROS_WARN("in bypass: bypass angle = %f", (BOOST_BYPASS_ANGLE / obstacle_edge) * turn_angle); // TODO

			// keep turning until front reaches obstacle's edge
			while(true) {

				ros::spinOnce();

				// check for emergency stop
				if (checkObstacle(EMERGENCY_STOP,EMERGENCY_STOP,EMERGENCY_STOP)) {
					ROS_ERROR("In ObstacleAvoider::run()- IN_BYPASS - EMERGENCY_STOP");
					m_driver->stop();
					sleep(1);
					continue;
				}

				// read current distances at front
				getRange(range);
				currDist = range[CENTER];

				// check if reached obstacle's edge
				if (std::abs(currDist - prevDist) > SIGNIFICAT_DIST_DIFF) {
					curr_state = STOP_BYPASS;
					break;
				}

				// keep turning until obstacle
				m_driver->turn(BOOST_BYPASS_ANGLE * turn_angle);
//				m_driver->turn((BOOST_BYPASS_ANGLE / obstacle_edge) * turn_angle); // TODO

				++counter;
			}

			break;
		}

		case STOP_BYPASS: {

			ROS_INFO("STOP BYPASS"); // TODO
			// stop turning
			m_driver->turn((-getSign(turn_angle)) * STOP_PREV_TURN);
			ROS_INFO("STOP BYPASS after turn"); // TODO

			// maintain current driving direction
			curr_state = MAINTAIN;
			ROS_INFO("STOP_BYPASS->MAINTAIN"); // TODO
			alarm(calcMaintainTime(obstacle_edge));
			break;
		}

		case MAINTAIN: {
			// maintaining driving direction
			break;
		}

		case RESET_ORIENTAION: {

			ROS_INFO("RESET");
			ROS_WARN("in reset: reset angle = %f. counter = %d", -BOOST_RESET_ANGLE * turn_angle,counter);

			// fix orientation as was before bypassing the obstacle
			float range[MAX_RANGE_SIZE];
			float currDist = 0.0;
			for (unsigned long i = 0 ; i < counter ; ++i) {

				ros::spinOnce();

				// check for emergency stop
				if (checkObstacle(EMERGENCY_STOP,EMERGENCY_STOP,EMERGENCY_STOP)) {
					ROS_ERROR("In ObstacleAvoider::run()- RESET - EMERGENCY_STOP");
					m_driver->stop();
					sleep(1);
					continue;
				}

				// simulate actions as done in IN_BYPASS in order to consume same time
				getRange(range);
				currDist = range[CENTER];

				// keep turning until orientation is corrected
				m_driver->turn(-BOOST_RESET_ANGLE * turn_angle);
			}

			// initialize counter for next time
			counter = 0;

			curr_state = STOP_RESET;
			break;
		}

		case STOP_RESET: {

			ROS_INFO("STOP RESET");
			// stop turning
			m_driver->turn((getSign(turn_angle)) * STOP_PREV_TURN);
			ROS_INFO("STOP RESET after turn");
			// keep running according to mode
			if (m_run_once) {
				return;
			}

			curr_state = START;
			break;
		}

		default:
			break;
		}
	}
}

void ObstacleAvoider::fixOrientation() {

	// stop robot
	m_driver->stop();

	// read current distances at front
	float range[MAX_RANGE_SIZE];
	getRange(range);
	float min_dist = range[CENTER], curr_dist = range[CENTER];
	short sign = 0;

	// find window with minimal average distance
	closestWindow(range, min_dist, sign);

	ROS_ERROR("in fixOrientation(): min_dist = %f, sign = %d", min_dist, sign);

	while(true) {

		ros::spinOnce();

		// check for emergency stop
		if (checkObstacle(EMERGENCY_STOP,EMERGENCY_STOP,EMERGENCY_STOP)) {
			ROS_ERROR("In ObstacleAvoider::fixOrientation()- EMERGENCY_STOP");
			m_driver->stop();
			sleep(1);
			continue;
		}

		// read current distances at front
		getRange(range);
		curr_dist = range[CENTER];

		// check if reached obstacle's edge
		if (std::abs(curr_dist - min_dist) <= TOLERANT) {
//			m_driver->turn(-sign * STOP_PREV_TURN); // TODO CONSIDER SIGN ACCORDING TO WINDOW'S LOCATION
			m_driver->stop(); // TODO DEBUG
//			m_driver->move(m_speed, 0.0);
			return;
		}

		// keep turning until obstacle
		m_driver->turn(sign * 0.4); // TODO CONSIDER SIGN ACCORDING TO WINDOW'S LOCATION
	}
}

void ObstacleAvoider::closestWindow(float range[], float& min_dist, short& sign) {

	// initialize current and minimal values on scan
	float curr_avg = 0.0;
	min_dist = avgElements(range,280,280+WINDOW_SIZE);

	// keep index of minimal window (indicates turn direction)
	int min_window_index = 0;

	// scan range from left to right
	for (int i = 280 ; i < 440-WINDOW_SIZE ; i += WINDOW_SIZE/2) {

		// calculate average distance at current window
		curr_avg = avgElements(range,i,i+WINDOW_SIZE);

		// update minimal value
		if (curr_avg <= min_dist) { // TODO CONSIDER "<"
			min_dist = curr_avg;
			min_window_index = i;
		}
	}

	// update sign according to deviation from range center
	sign = (min_window_index > CENTER) ? LEFT_SIGN : RIGHT_SIGN;
}

void ObstacleAvoider::bypassObstacle(float& turn_angle, float& obstacle_edge) {

	float right_edge = 0.0, left_edge = 0.0;
	int right_edge_index = RIGHT_RANGE_END, left_edge_index = LEFT_RANGE_END,
			min_diff = 0;
	float range[MAX_RANGE_SIZE];
	bool right_edge_detected = false;
	bool left_edge_detected = false;

	// read current distances at front
	ros::spinOnce();
	getRange(range);

	// check for obstacle's right edge
	right_edge_detected = findRightEdge(range, right_edge_index, right_edge);
	// check for obstacle's left edge
	left_edge_detected = findLeftEdge(range, left_edge_index, left_edge);

	// if "left" and "right" not detected the edge
	if (false == right_edge_detected && false == left_edge_detected) {
		return;
	}

	// find difference between nearest clear edge and middle position // TODO EXPLAIN THE SIGNS
	if ((CENTER - right_edge_index) <= (left_edge_index - CENTER)) {

		min_diff = right_edge_index - CENTER;
		obstacle_edge = right_edge;

	} else {

		min_diff = left_edge_index - CENTER;
		obstacle_edge = left_edge;
	}

	// calculate turn according to the difference
	double distToEdge =	right_edge_detected ?
					range[right_edge_index] : range[left_edge_index];
	turn_angle = calcTurnAngle(min_diff, distToEdge);

	ROS_WARN("in bypassObstacle, turn_angle = %f, min_diff = %d, distToEdge = %f", turn_angle, min_diff, distToEdge);
}

bool ObstacleAvoider::findRightEdge(float range[], int& right_edge_index,
		float& right_edge) {

	float curr_dist = 0.0;
	right_edge = range[RIGHT_RANGE_START];
	for (int i = RIGHT_RANGE_START- 1; i >= RIGHT_RANGE_END; --i) {

		curr_dist = range[i];

		if ((curr_dist - right_edge) > SIGNIFICAT_DIST_DIFF) {
			right_edge_index = i;
			return true;
		}

		right_edge = curr_dist;
	}

	return false;
}

bool ObstacleAvoider::findLeftEdge(float range[], int& left_edge_index,
		float& left_edge) {

	float curr_dist = 0.0;
	left_edge = range[LEFT_RANGE_START];
	for (int i = LEFT_RANGE_START + 1; i < LEFT_RANGE_END; ++i) {

		curr_dist = range[i];

		if ((curr_dist - left_edge) > SIGNIFICAT_DIST_DIFF) {
			left_edge_index = i;
			return true;
		}

		left_edge = curr_dist;
	}

	return false;
}

/**
 * Checks if there is an obstacle at min_dist meters ahead
 */
bool ObstacleAvoider::checkObstacle(float min_right, float min_center, float min_left) {

	// find minimal distance from front range
	float right, center, left;
	getBins(right, center, left);

	// check each direction
	if ((right <= min_right) || (center <= min_center) || (left <= min_left)) {
		return true;
	}

	return false;
}

short ObstacleAvoider::getSign(float x) {
	return (short) (x / std::abs(x));
}

float ObstacleAvoider::getMin(float x, float y, float z) {
	float min = (x <= y) ? x : y;
	return (min <= z) ? min : z;
}

float ObstacleAvoider::calcTurnAngle(int diff, float distToEdge) {

	if (0 == diff) {
		return 0.0;
	}

	int sign = (int) diff / std::abs(diff);
	float angle = diff / (DIFF_TO_ANGLE * distToEdge);
	return (std::abs(angle) < 0.2) ? (angle + sign * LINEAR_BOOST) : angle;
}

/*
 * obstacle edge is the distance to the edge the robot turns to
 */
int ObstacleAvoider::calcMaintainTime(float obstacle_edge) {
	int time = (int) (obstacle_edge / m_speed) - 1;
	ROS_WARN("in calcMaintainTime. time = %d", time);
	return (0 == time) ? 1 : time;
}

void ObstacleAvoider::getBins(float& right, float& center, float& left) {

	std::unique_lock<std::mutex> mlock(m_mutex);
	right = m_right_bin;
	center = m_center_bin;
	left = m_left_bin;
	mlock.unlock();
}

void ObstacleAvoider::setBins(float right, float center, float left) {

	std::unique_lock<std::mutex> mlock(m_mutex);
	m_right_bin = right;
	m_center_bin = center;
	m_left_bin = left;
	mlock.unlock();
}

void ObstacleAvoider::getRange(float range[]) {

	std::unique_lock<std::mutex> mlock(m_mutex);
	for (int i = 0; i < MAX_RANGE_SIZE; ++i) {
		range[i] = m_range[i];
	}
	mlock.unlock();
}

void ObstacleAvoider::setRange(
		const std_msgs::Float32MultiArray::ConstPtr& range) {

	std::unique_lock<std::mutex> mlock(m_mutex);
	for (int i = 0; i < MAX_RANGE_SIZE; ++i) {
		m_range[i] = range->data[i];
	}
	mlock.unlock();
}
